import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.Sound;

public class Controller extends Thread {
	private DistanceMonitor frontMonitor;
	private DistanceMonitor rearMonitor;
	private DistanceMonitor sideMonitor;
	private ParkingSpot spot;
	private static final int SPEED = 180;
	private static final int RIGHT = 35;
	private static final int LEFT = -35;
	int itAdju = 0;
	int itBack = 0;

	public Controller(DistanceMonitor frontMonitor,
			DistanceMonitor rearMonitor, DistanceMonitor sideMonitor) {
		this.frontMonitor = frontMonitor;
		this.rearMonitor = rearMonitor;
		this.sideMonitor = sideMonitor;
		spot = new ParkingSpot();
	}

	public void run() {
		if (findParkingSpot()) {
			enterParkingSpot();
			while (sideMonitor.getDistance() > 8) {
				adjustDepthInTheParkingSpot();
			}
			setCarInCenter();
			Sound.setVolume(100);
			Sound.systemSound(false, 2);
			Button.waitForPress();
			
		} else {
			LCD.clear();
			System.out.println("Parking spot is too small!");
			Button.waitForPress();
		}
		Motor.A.stop();
		System.exit(0);
	}

	private boolean findParkingSpot() {
		double stop;
		Motor.A.setSpeed(SPEED);
		Motor.B.rotateTo(0);
		superSleep(110);
		Motor.A.forward();
		while (sideMonitor.getDistance() < 15) {
			superSleep(100);
		}
		Motor.A.resetTachoCount();
		
		superSleep(200);
		spot.setDepth(sideMonitor.getDistance());
		superSleep(100);
		Motor.A.forward();
		while (sideMonitor.getDistance() > 15) {
			superSleep(100);
		}
		Motor.A.stop();
		stop = Motor.A.getTachoCount();
		spot.setLength((int) ( (stop/ 360) * 175.929));
		superSleep(110);
		Motor.A.setSpeed(0);
		if (spot.getLength() > 300) {
			return true;
		}
		return false;
	}

	private void enterParkingSpot() {
		superSleep(1000);
		Motor.A.setSpeed(SPEED);
		int rotateLength = 0;
		if(sideMonitor.getDistance() > 6) {
			rotateLength = (int) (sideMonitor.getDistance() * 360 / (17.6*2));
			Motor.A.rotate(rotateLength);
		}
		superSleep(200);
		Motor.B.rotateTo(RIGHT);
		superSleep(200);
		Motor.A.resetTachoCount();
		superSleep(100);
		rotateLength = -(spot.getLength()+ rotateLength) * 360 / (176 * 2);
		Motor.A.backward();
		while (Motor.A.getTachoCount() > rotateLength * 1.10) {
			superSleep(100);
		}
		Motor.A.stop();
		Motor.B.rotateTo(LEFT);
		Motor.A.resetTachoCount();
		Motor.A.backward();
		while (Motor.A.getTachoCount() > rotateLength * 1.10) {
			superSleep(100);
		}
		Motor.A.stop();
		Motor.B.rotateTo(0);
		forward();
		backward();
	}

	private void setCarInCenter() {
		Motor.A.setSpeed(SPEED);
		superSleep(110);
		Motor.A.forward();
		while (frontMonitor.getDistance() > rearMonitor.getDistance()) {
			superSleep(110);
		}
		Motor.A.stop();
	}

	private void adjustDepthInTheParkingSpot() {
		itAdju = itAdju + 1;
		int distance = frontMonitor.getDistance() * 10;
		Motor.A.setSpeed(SPEED);
		Motor.B.rotateTo(0);
		superSleep(100);

		// forward correction
		Motor.B.rotateTo(RIGHT);
		superSleep(110);
		Motor.A.resetTachoCount();
		Motor.A.forward();
		while (((double) Motor.A.getTachoCount() / 360) * 175.929 < (distance-70) / 2) {
			superSleep(100);
		}
		Motor.A.stop();
		Motor.B.rotateTo(LEFT);
		superSleep(100);
		Motor.A.resetTachoCount();
		Motor.A.forward();
		while (((double) Motor.A.getTachoCount() / 360) * 175.929 < (distance-70) / 2) {
			superSleep(100);
		}
		Motor.A.stop();
		backward();
	}

	private void forward() {
		Motor.B.rotateTo(0);
		if (frontMonitor.getDistance() > 10) {
			Motor.A.forward();
		}
		while (frontMonitor.getDistance() > 10) {
			superSleep(100);
		}
		Motor.A.stop();
	}

	private void backward() {
		int distance = sideMonitor.getDistance();
		Motor.B.rotateTo(0);
		if (rearMonitor.getDistance() > 7) {
			Motor.A.backward();
			while (rearMonitor.getDistance() > 7) {
				superSleep(100);
			}
			superSleep(300);
			Motor.A.stop();
			Motor.B.rotateTo(0);
		}
		int diff = sideMonitor.getDistance() - distance;
		if(Math.abs(diff) > 1 && Math.abs(diff) < 6){
			itBack = itBack + 1;
			if(diff > 0){
				Motor.B.rotateTo(LEFT);
				superSleep(110);
				Motor.A.rotate(diff * 15);
				superSleep(500);
				Motor.B.rotateTo(RIGHT);
				superSleep(110);
				Motor.A.rotate(-diff * 15);
			} else if (diff < 0){
				Motor.B.rotateTo(RIGHT);
				superSleep(110);
				Motor.A.rotate(-diff * 15);
				superSleep(500);
				Motor.B.rotateTo(LEFT);
				Motor.A.rotate(diff * 15);
			}
			superSleep(500);
			Motor.B.rotateTo(0);
			superSleep(100);
		}
	}

	private void superSleep(int milliseconds) {
		try {
			Thread.sleep(milliseconds);
		} catch (InterruptedException e) {
			e.printStackTrace();
		}
	}

}